# coding=utf-8
from numpy import *
import matplotlib.pyplot as plt


# 向量转换 返回旋转后的向量
def Transform2D(ang, vector):
    Zeropoint = array([0, 0])
    for i in arange(0, ang + 1, 2):
        ax = plt.axes()
        ax.plot(*Zeropoint, 'or')
        rad = deg2rad(i)
        ax.arrow(*Zeropoint, vector[0, 0], vector[1, 0], color='g', linewidth=2.0, head_width=0.08, head_length=0.1)

        # 二维旋转矩阵
        TransformM = mat([
            [cos(rad), -sin(rad)],
            [sin(rad), cos(rad)]
        ])
        NewV = TransformM * vector
        ax.arrow(*Zeropoint, NewV[0, 0], NewV[1, 0], color='r', linewidth=2.0, head_width=0.08, head_length=0.1)

        plt.text(vector[0, 0], vector[1, 0], "(%.3f,%.3f)" % (vector[0, 0], vector[1, 0]), color='k')
        plt.text(NewV[0, 0], NewV[1, 0], "(%.3f,%.3f)" % (NewV[0, 0], NewV[1, 0]), color='k')
        plt.text(0.05, -0.05, "(ang %d)" % (i), color='k')
        plt.axis('equal')
        plt.grid(b=True, which='major')
        if i == ang:
            plt.show()
            return NewV
        else:
            plt.show(block=False)
            plt.pause(0.1)
            plt.clf()


# 坐标系转换
def TransformCoorSys2d(ang):
    Zeropoint = array([0, 0])
    Xpoint = mat([[0], [1]])
    Ypoint = mat([[1], [0]])
    for i in arange(0, ang + 1, 1):
        ax = plt.axes()
        ax.plot(*Zeropoint, 'or')

        rad = deg2rad(i)
        # 原始
        ax.arrow(*Zeropoint, Xpoint[0, 0], Xpoint[1, 0], color='g', linewidth=2.0, head_width=0.20, head_length=0.25)
        ax.arrow(*Zeropoint, Ypoint[0, 0], Ypoint[1, 0], color='g', linewidth=2.0, head_width=0.20, head_length=0.25)

        # 列向量
        TransformM = mat([
            [cos(rad), -sin(rad)],
            [sin(rad), cos(rad)],
        ])
        NewX = TransformM * Xpoint
        NewY = TransformM * Ypoint

        ax.arrow(*Zeropoint, NewX[0, 0], NewX[1, 0], color='r', linewidth=2.0, head_width=0.20, head_length=0.25)
        ax.arrow(*Zeropoint, NewY[0, 0], NewY[1, 0], color='r', linewidth=2.0, head_width=0.20, head_length=0.25)

        plt.text(NewX[0, 0], NewX[1, 0], "(%.3f,%.3f)" % (NewX[0, 0], NewX[1, 0]), color='k')
        plt.text(NewY[0, 0], NewY[1, 0], "(%.3f,%.3f)" % (NewY[0, 0], NewY[1, 0]), color='k')

        plt.text(0.05, -0.05, "(ang %d°)" % (i), color='k')
        plt.axis('equal')
        plt.grid(b=True, which='major')
        if i == ang:
            plt.show()
        else:
            plt.show(block=False)
            plt.pause(0.01)
            plt.clf()


def TransformVector3d(Xang, Yang, Zang, Vextor3D):
    radx = deg2rad(Xang)
    rady = deg2rad(Yang)
    radz = deg2rad(Zang)
    XtransformM = mat([
        [1, 0, 0],
        [0, cos(radx), -sin(radx)],
        [0, sin(radx), cos(radx)]
    ])
    YtransformM = mat([
        [cos(rady), 0, sin(rady)],
        [0, 1, 0],
        [-sin(rady), 0, cos(rady)]
    ])
    ZtransformM = mat([
        [cos(radz), -sin(radz), 0],
        [sin(radz), cos(radz), 0],
        [0, 0, 1]
    ])
    tmp = XtransformM * YtransformM * ZtransformM * Vextor3D
    return tmp


def TransformV3dShow(Xang, Yang, Zang, Vextor3D):
    Zeropoint = array([0, 0, 0])
    for i in arange(0, Xang + Yang + Zang + 1, 1):
        ax = plt.axes(projection='3d')
        ax.quiver(*Zeropoint, 1, 0, 0, length=1.0, color="r")
        ax.quiver(*Zeropoint, 0, 1, 0, length=1.0, color="g")
        ax.quiver(*Zeropoint, 0, 0, 1, length=1.0, color="b")
        ax.quiver(*Zeropoint, Vextor3D[0, 0], Vextor3D[1, 0], Vextor3D[2, 0], length=1.0, color="k")
        radx = rady = radz = 0

        if i <= Xang:
            radx = i
        elif i > Xang and i <= Xang + Yang:
            radx = Xang
            rady = i - Xang
        elif i > Xang + Yang:
            radx = Xang
            rady = Yang
            radz = i - Xang - Yang

        NewV = TransformVector3d(radx, rady, radz, Vextor3D)

        ax.quiver(*Zeropoint, NewV[0, 0], NewV[1, 0], NewV[2, 0], length=1.0, color="b")
        plt.title("Xang %.3f Yang %.3f Zang %.3f" % (radx, rady, radz))
        plt.plot(*Zeropoint, 'ro')
        ax.set_xlim(-1, 1)
        ax.set_ylim(-1, 1)
        ax.set_zlim(-1, 1)
        if i == Xang + Yang + Zang:
            plt.show()
        else:
            plt.show(block=False)
            plt.pause(0.1)
            plt.clf()


# TransformCoorSys2d(20)
v1 = mat([
    [0],
    [1],
    [0],
])

TransformV3dShow(0, 0, 90, v1)
